#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=5118;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

Eigen::Matrix4f P2_, Trv2c_;

ros::Publisher cloud_pub_;

void pc_range_handle(pcl::PointCloud<PointType>::Ptr p_pc){
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*p_pc, *p_pc, indices);
    pcl::PassThrough<PointType> pass_filter;
    pass_filter.setInputCloud (p_pc);
    pass_filter.setFilterFieldName ("z");
    pass_filter.setFilterLimits (-1.2, 0);
    pass_filter.setFilterLimitsNegative (false);
    pass_filter.filter (*p_pc);

    pass_filter.setInputCloud (p_pc);
    pass_filter.setFilterFieldName ("y");
    pass_filter.setFilterLimits (-10,10);
    pass_filter.setFilterLimitsNegative (false);
    pass_filter.filter (*p_pc);

    pass_filter.setInputCloud (p_pc);
    pass_filter.setFilterFieldName ("x");
    pass_filter.setFilterLimits (0, 20);
    pass_filter.setFilterLimitsNegative (false);
    pass_filter.filter (*p_pc);
}


void cloudCB(const sensor_msgs::PointCloud2ConstPtr& pc_msg)
{
    pcl::PointCloud<PointType>::Ptr pc_(new pcl::PointCloud<PointType>);
    pcl::fromROSMsg(*pc_msg, *pc_);
    pc_range_handle(pc_);
    sensor_msgs::PointCloud2 pc_display;
    pcl::toROSMsg(*pc_,pc_display);
    pc_display.header=pc_msg->header;
    cloud_pub_.publish(pc_display);
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;
    ros::Subscriber pc_sub =  nh.subscribe("/velodyne_points", 10, cloudCB);
    cloud_pub_=nh.advertise<sensor_msgs::PointCloud2>("velodune_points_cut",1);

    ros::spin();

    return 0;
}
